package org.maxgroundstation.groundstation;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import net.java.games.input.Component;
import net.java.games.input.Controller;
import net.java.games.input.ControllerEnvironment;
import net.java.games.input.Keyboard;

/**
 * Controller poller for joystick and keyboard devices.
 * @author Scott Davis
 * 
 * Copyright (C) 2010  Scott Davis
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
public class ControllerInputPoller implements Runnable {

    // Length of polling thread sleep time (milliseconds).
    private static final long SLEEP_TIME = 100L;
    
    // Turret rotation rate in radians/sec.
    private static final float TURRET_ROTATION_RATE = 1F;
    
    // Motor acceleration rate 0 -> full speed / seconds.
    private static final float MOTOR_ACCELERATION_RATE = 2F;
    
    private GroundStation gStation;
    private Controller joystick;
    private List<Controller> keyboards;
    private boolean run;
    private Thread pollingThread;
    private long lastControllerDetection;
    private boolean useJoystick;
    private float masterGovernor;
    private boolean firstPoll;
    
    // Data caches for input values.
    private float xAxisCache = 0F;
    private float yAxisCache = 0F;
    private float rzAxisCache = 0F;
    private float sliderCache = 1F;
    private float povYCache = 0F;
    private float povXCache = 0F;
    private float keyMotorCache = 0F;
    private float keyGovernorCache = 1F;
    private float wheelsTurnCache = 0F;
    
    /**
     * Constructor
     * @param gStation the ground station.
     */
    ControllerInputPoller(GroundStation gStation) {
        
        this.gStation = gStation;
        joystick = null;
        keyboards = new ArrayList<Controller>();
        run = true;
        pollingThread = null;
        lastControllerDetection = 0L;
        useJoystick = false;
        masterGovernor = .5F;
        firstPoll = true;
        
        // Set property for polling keyboards.
        System.setProperty("jinput.plugins", "net.java.games.input.AWTEnvironmentPlugin");
        
        // Start refresh thread.
        start();
    }
    
    /**
     * Sets the refresh thread to run or not.
     * @param run true if panel is to be refreshed.
     */
    public void setRun(boolean run) {
        this.run = run;
    }
    
    /**
     * Starts the update thread.
     */
    public void start() {
        if (pollingThread == null) 
            pollingThread = new Thread(this, "Polling thread");
        pollingThread.start();
    }
    
    /**
     * Run the update thread.
     */
    public void run() {
        while (run) {
            try {
                Thread.sleep(SLEEP_TIME);
            } 
            catch (InterruptedException e) {}
                
            // Detect input controllers once a second.
            long currentTime = System.currentTimeMillis();
            if ((currentTime - lastControllerDetection) > 1000L) {
                detectInputControllers();
                lastControllerDetection = currentTime;
            }
                
            // Poll the input controllers.
            pollInputControllers();
        }
    }
    
    /**
     * Gets the master governor setting.
     * @return master governor setting (0.0 - 1.0).
     */
    public float getMasterGovernor() {
        return masterGovernor;
    }
    
    /**
     * Sets the master governor setting.
     * @param newMasterGovernor the new master governor setting (0.0 - 1.0).
     */
    public void setMasterGovernor(float newMasterGovernor) {
        masterGovernor = newMasterGovernor;
        if (masterGovernor < 0F) masterGovernor = 0F;
        if (masterGovernor > 1F) masterGovernor = 1F;
    }
    
    /**
     * Check if joystick is available.
     * @return true if joystick available.
     */
    public boolean hasJoystick() {
        return (joystick != null);
    }
    
    /**
     * Check if mouse should be used to control robot.
     * @return true if use mouse.
     */
    public boolean useMouse() {
        return !useJoystick;
    }
    
    /**
     * Set if joystick should be used to control robot.
     * @param useJoystick true if use joystick
     */
    public void setUseJoystick(boolean useJoystick) {
        System.out.println("setUseJoystick: " + useJoystick);
        this.useJoystick = useJoystick;
    }
    
    /**
     * Detect all input controller devices on system.
     */
    private void detectInputControllers() {
        
        Controller oldJoystick = joystick;
        joystick = null;
        keyboards.clear();
        
        // Create a list of each type of controller.
        Controller[] ca = ControllerEnvironment.getDefaultEnvironment().getControllers();
        for (int x = 0; x < ca.length; x++) {
            if (ca[x].getType().equals(Controller.Type.STICK)) joystick = ca[x];
            else if (ca[x].getType().equals(Controller.Type.KEYBOARD)) keyboards.add(ca[x]);
        }
        
        if (oldJoystick != joystick) {
            gStation.fireGroundStationEvent(GroundStationEvent.CONTROLLERS_DETECTED);
        }
    }
    
    /**
     * Poll controller devices.
     */
    private void pollInputControllers() {
        // Check if communication socket is connected.
        if (gStation.getCommandSender().isConnected()) {
            if (useJoystick) pollJoystick();
            else pollKeyboards();
            
            if (firstPoll) firstPoll = false;
        }
        else firstPoll = true;
    }
    
    /**
     * Poll joystick controller device.
     */
    private void pollJoystick() {
        
        // Poll the joystick.
        joystick.poll();
            
        Component slider = joystick.getComponent(Component.Identifier.Axis.SLIDER);
        boolean sliderUpdated = false;
        if (slider != null) {
            float sliderValue = slider.getPollData();
            if (sliderValue != sliderCache) {
                sliderCache = sliderValue;
                sliderUpdated = true;
            }
        }
        
        boolean xAxisChange = false;
        Component xAxis = joystick.getComponent(Component.Identifier.Axis.X);
        if (xAxis != null) {
            float xAxisValue = xAxis.getPollData();
            if (xAxisValue != xAxisCache) {
                float modXAxisValue = -1F * xAxisValue * MaxConstants.MAX_REAR_WHEELS_TURN_ANGLE_LIMIT;
                if (!firstPoll) gStation.getCommandSender().turnRear(modXAxisValue);
                xAxisChange = true;
                xAxisCache = xAxisValue;
            }
        }
            
        boolean yAxisChange = false;
        Component yAxis = joystick.getComponent(Component.Identifier.Axis.Y);
        if (yAxis != null) {
            float yAxisValue = yAxis.getPollData();
            if ((yAxisValue != yAxisCache) || sliderUpdated) {
                
                // Dampen motor acceleration to motor acceleration rate.
                float accelerationRate = MOTOR_ACCELERATION_RATE / (1000L / SLEEP_TIME);
                accelerationRate /= ((1F - sliderCache) / 2F) * masterGovernor;
                if ((yAxisValue - yAxisCache) > accelerationRate) {
                    yAxisValue = yAxisCache + accelerationRate;
                    //System.out.println("accelerationRate: " + accelerationRate);
                }
                else if ((yAxisCache - yAxisValue) > accelerationRate) {
                    yAxisValue = yAxisCache - accelerationRate;
                }
                
                float modYAxisValue = (0F - yAxisValue) * ((1F - sliderCache) / 2F) * masterGovernor;
                if (!firstPoll) gStation.getCommandSender().driveMotor(modYAxisValue);
                //System.out.println("modYAxisValue: " + modYAxisValue);
                yAxisChange = true;
                yAxisCache = yAxisValue;
            }
        }
            
        boolean rzAxisChange = false;
        Component rzAxis = joystick.getComponent(Component.Identifier.Axis.RZ);
        if (rzAxis != null) {
            float rzAxisValue = rzAxis.getPollData();
            if (rzAxisValue != rzAxisCache) {
                float modRZAxisValue = rzAxisValue * MaxConstants.MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT;
                if (!firstPoll) gStation.getCommandSender().turnFront(modRZAxisValue);
                rzAxisChange = true;
                rzAxisCache = rzAxisValue;
            }
        }
            
        // Set the camera turret pan and tilt if camera drive lock is being used 
        // and rover is driving forwards.
        if (gStation.getCameraDriveLock() && (yAxisCache <= -.05F)) {
        	
        	// Set tilt to zero if not already.
        	if (povYCache != 0F) {
        		if (!firstPoll) gStation.getCommandSender().tilt(0F);
    			povYCache = 0F;
        	}
        	
        	// Set pan to match rover's direction.
        	if (xAxisChange || yAxisChange || rzAxisChange) {
        		float frontWheelValue = rzAxisCache * MaxConstants.MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT;
        		float rearWheelValue = -1F * xAxisCache * MaxConstants.MAX_REAR_WHEELS_TURN_ANGLE_LIMIT;
        		float panValue = 0F;
        		if (yAxisCache > 0F) {
        			panValue = .5F * ((float) Math.PI + (rearWheelValue - frontWheelValue));
        		}
        		else if (yAxisCache < 0F) {
        			panValue = .5F * (frontWheelValue - rearWheelValue);
        		}
        		if (!firstPoll) gStation.getCommandSender().pan(panValue);
    			povXCache = panValue;
        	}
        }
        else {
        	// Move camera turret based on joystick input.
        	Component pov = joystick.getComponent(Component.Identifier.Axis.POV);
        	if (pov != null) {
        		float angleChange = (float) SLEEP_TIME / 1000F * TURRET_ROTATION_RATE;
            
        		float povValue = pov.getPollData();
        		float newPovX = povXCache;
        		float newPovY = povYCache;
            
        		if ((povValue == Component.POV.UP_LEFT) || (povValue == Component.POV.LEFT) || 
        				(povValue == Component.POV.DOWN_LEFT)) {
        			newPovX = povXCache - angleChange;
        		}
            
        		if ((povValue == Component.POV.UP_RIGHT) || (povValue == Component.POV.RIGHT) || 
        				(povValue == Component.POV.DOWN_RIGHT)) {
        			newPovX = povXCache + angleChange;
        		}
            
        		if ((povValue == Component.POV.UP_LEFT) || (povValue == Component.POV.UP) || 
        				(povValue == Component.POV.UP_RIGHT)) {
        			newPovY = povYCache - angleChange;
        			if (newPovY > MaxConstants.MAX_HEAD_TILT_ANGLE) newPovY = MaxConstants.MAX_HEAD_TILT_ANGLE;
        		}
            
        		if ((povValue == Component.POV.DOWN_LEFT) || (povValue == Component.POV.DOWN) || 
        				(povValue == Component.POV.DOWN_RIGHT)) {
        			newPovY = povYCache + angleChange;
        			if (newPovY < MaxConstants.MIN_HEAD_TILT_ANGLE) newPovY = MaxConstants.MIN_HEAD_TILT_ANGLE;
        		}
            
        		if (newPovX != povXCache) {
        			if (!firstPoll) gStation.getCommandSender().pan(newPovX);
        			povXCache = newPovX;
        		}
                
        		if (newPovY != povYCache) {
        			if (!firstPoll) gStation.getCommandSender().tilt(newPovY);
        			povYCache = newPovY;
        		}
        	}
        }
    }
    
    /**
     * Poll all keyboard devices available.
     */
    private void pollKeyboards() {
        
        float keyMotor = 0F;
        float keyGovernor = keyGovernorCache;
        float wheelsTurn = 0F;
        
        Iterator<Controller> i = keyboards.iterator();
        while (i.hasNext()) {
            Keyboard keyboard = (Keyboard) i.next();
            keyboard.poll();
            
            if (keyboard.isKeyDown(Component.Identifier.Key.UP)) keyMotor = 1F;
            else if (keyboard.isKeyDown(Component.Identifier.Key.W)) keyMotor = 1F;
            else if (keyboard.isKeyDown(Component.Identifier.Key.DOWN)) keyMotor = -1F;
            else if (keyboard.isKeyDown(Component.Identifier.Key.S)) keyMotor = -1F;
            
            if (keyboard.isKeyDown(Component.Identifier.Key._1)) keyGovernor = .1F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._2)) keyGovernor = .2F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._3)) keyGovernor = .3F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._4)) keyGovernor = .4F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._5)) keyGovernor = .5F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._6)) keyGovernor = .6F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._7)) keyGovernor = .7F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._8)) keyGovernor = .8F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._9)) keyGovernor = .9F;
            else if (keyboard.isKeyDown(Component.Identifier.Key._0)) keyGovernor = 1F;
            
            if (keyboard.isKeyDown(Component.Identifier.Key.LEFT)) wheelsTurn = -1F;
            else if (keyboard.isKeyDown(Component.Identifier.Key.A)) wheelsTurn = -1F;
            else if (keyboard.isKeyDown(Component.Identifier.Key.RIGHT)) wheelsTurn = 1F;
            else if (keyboard.isKeyDown(Component.Identifier.Key.D)) wheelsTurn = 1F;
        }
        
        // Send motor command if necessary.
        boolean motorChange = false;
        if ((keyMotor != keyMotorCache) || (keyGovernor != keyGovernorCache)) {
            
            // Dampen motor acceleration to motor acceleration rate.
            float accelerationRate = MOTOR_ACCELERATION_RATE / (1000L / SLEEP_TIME);
            accelerationRate /= keyGovernor * masterGovernor;
            if ((keyMotor - keyMotorCache) > accelerationRate) keyMotor = keyMotorCache + accelerationRate;
            else if ((keyMotorCache - keyMotor) > accelerationRate) keyMotor = keyMotorCache - accelerationRate;
            
            float motorValue = keyMotor * keyGovernor * masterGovernor;
            if (!firstPoll) gStation.getCommandSender().driveMotor(motorValue);
            keyMotorCache = keyMotor;
            keyGovernorCache = keyGovernor;
            motorChange = true;
        }
        
        // Send wheels turn commands if necessary.
        boolean wheelsChange = false;
        if (wheelsTurn != wheelsTurnCache) {
            float frontTurnAngle = wheelsTurn * MaxConstants.MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT;
            if (!firstPoll) gStation.getCommandSender().turnAntiRoll(frontTurnAngle);
            wheelsTurnCache = wheelsTurn;
            wheelsChange = true;
        }
        
        // If camera drive lock is set and driving forwards, move camera turret with wheel turn.
        if (gStation.getCameraDriveLock() && (keyMotorCache >= .05F)) {
        	float tempMotorValue = keyMotorCache * keyGovernorCache * masterGovernor;
            float tempFrontTurnAngle = wheelsTurnCache * MaxConstants.MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT;
        	
        	// Set tilt to zero if not already.
            if (!firstPoll) gStation.getCommandSender().tilt(0F);
        	
        	// Set pan to match rover's direction.
        	if (motorChange || wheelsChange) {
        		float panValue = 0F;
        		if (tempMotorValue > 0F) {
        			panValue = tempFrontTurnAngle;
        		}
        		else if (tempMotorValue < 0F) {
        			panValue = (float) Math.PI - tempFrontTurnAngle;
        		}
        		if (!firstPoll) gStation.getCommandSender().pan(panValue);
        	}
        }
    }
}